#ifndef _C_VIEW_2D_H_
#define _C_VIEW_2D_H_

#include <iostream>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>

#include "SMATModel.hh"

#include "pose3d.h"
#include "stereo.h"
#include "ransac.h"
#include <boost/circular_buffer.hpp>

#define TRACK 1


class CModel3D;

class CView2D : public CView {
 public:
    int npoints;

    pose3D pose_old;
    
    CvMat * points2d;
    CvMat * points2d_T;
    CvMat points2d2;
    CvMat * points2d_reproject;
    CvMat * pointstemp;
    CvMat *dist1D;
    
    CvMat *vect_media;
    CvMat *translation_media;

    CvMat * offsettrans, *offsettrans2;
    float *x2, *y2, *z2;

    CvMat *maskout;
    CvMat *maskoutliers;
    CvMat *masktemp;
    CvMat *maskout2d;
    CvMat *maskset;


    int inliers;
    float error;
    CvScalar offsetmodel;
    
    CvMat * offsetvect;
    float ai, bi, gi;
    double disttrans;  

    CModel3D * p_linkmodel;

    //params for caclPose
    CvTermCriteria criteria;
    double flFocalLength;
    double flx0;
    double fly0;
    CvMat *T_temp;

    bool m_alloc;

    //smat staff
    CvSize size_smat_ori;
    CvSize size_smat_dest;
    IplImage * img_smat;
    IplImage * imgtemp_smat;
    boost::shared_ptr<SMATModel> smat;
    CvRect roiori;
    CvRect roidest;
    CvScalar desplazar_parche;
    int smat_patch_size;
    float factor;

    //tracking
    CvFont fontmod;
    pose3D oldpose_track;
    boost::circular_buffer<IplImage*> patches;
    boost::circular_buffer<IplImage*> newpatches;
    boost::circular_buffer<IplImage*> lastpatches;
    CvMat * matchvals;
    CvSize matchsizes[3];
    IplImage *result;
    CvMat *suma;
    CvMat *matchpatchused;


 public:

    void calcPose(CvMat *maskin = NULL);

    CView2D()
    {
	m_alloc = false;
	points2d=0;
	points2d_T = 0;
	vect_media = cvCreateMat(3,1, CV_32FC1);
	translation_media = cvCreateMat( 3, 1, CV_32FC1 );
	T_temp = cvCreateMat(3,1,CV_32FC1);
	matchvals =0;
	result =0;
	suma =0;
	matchpatchused =0;

	double hScale=0.4, vScale = 0.4;
	int lineWidth = 1;
	cvInitFont(&fontmod, CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, hScale,vScale,0,lineWidth);
    }

    CView2D(CModel3D & pmodel, CALIBRATION_PARAMS_t *calibration_params)
    {
	CView2D();
	linkmodel(pmodel, calibration_params);
    }

    ~CView2D()
    {
	cvReleaseMat(&vect_media);
	cvReleaseMat(&translation_media);
	cvReleaseMat(&T_temp);

	//TODO:
	//hay más cosas por borrar...
    }


    void ChangeView(CView2D &pv2);
    void set_points(int points);
    void set2z();
    void linkmodel(CModel3D &pmodel, CALIBRATION_PARAMS_t *calibration_params );
    void alloc(int points);
    void dealloc();
    void display_SMATModel(IplImage *img);
    int ReadModelFromFile(const std::string filename, int n, CvRect roi=cvRect(0,0,0,0));
    void SaveModel(CvFileStorage * fs, int n, CvRect roi);
    void SaveModel(const std::string filename, int n, CvRect roi);
    void reproject();
    void reproject2();
    int deletepoint(int index);
    int closer_point(int x, int y);
    CvMat * Rneg();
    void getNotHiden();
    void reinit_SMAT(const char * model=NULL, int size=19);
    void init_SMAT(CvSize orisize, const char * model=NULL, int size=38);
    int addpoint();
    int SMAT ();
    void preprocess_smat_image(IplImage *img);
    int RANSAC_posit(ransac_params_t *);
    void RANSAC_results(IplImage *img=0);
    int GetInliersError (CvMat * maskin, CvMat *v_outliers, float *errori);

    void display_track(IplImage * img_track, IplImage * img);
    void tracker(IplImage * img, CvSize size);
    void track_old(IplImage * img);
    void init_track(IplImage * img);
    void update_track(IplImage * img);
    CvPoint trackpoint(IplImage * img, int i, CvSize size);
    CvPoint Point(int i);
    void add_trackpoint(int i, IplImage * img);
    void update_trackpoint(int i, IplImage *img, bool updatenewpatch);
    CvPoint trackpoint(CvPoint pt1, IplImage *img, IplImage * oldimg, CvSize size, float * val);
    CvRect PointRect(CvPoint pt0);
    CvRect PointRect(int i);

};




#endif
